#pragma once

#include <vector>
#include "../geometry/i_twoview.h"
#include "../da/i_ransac.h"
#include "../optimize/i_lm.h"

namespace idl
{
  /*This routine verifies the epipolar geometry and filter out matching outliers*/
  template<typename InterestPointType, typename DescriptorType>
  inline int i_correspondences_epipolar_geometry_verification(const std::vector<Pair<InterestPointType, InterestPointType> >& correspondences, std::vector<bool>& survivors, double epi_dist_thre, double *fundamental = NULL)
  {
    if (correspondences.empty())
    {
      return 0;
    }
    int i, j, nr_inlier_epi_geometry = 0, survivor_count = 0;
    int nr_2d_matches = (int)correspondences.size();
    double f[9], error_mean, error_sdv, error_thresh;
    if (fundamental != NULL)
    {
      i_eye_3x3(fundamental);
    }
    survivors.resize(nr_2d_matches, false); /*initialization - no survivors*/
    std::vector<int> inliers(nr_2d_matches * 2);
    std::vector<double> xs(nr_2d_matches * 3, (double)1.0);
    std::vector<double> xps(nr_2d_matches * 3, (double)1.0);
    std::vector<double> epi_errors;

    /*start normalized eight points algorithm*/
    for (i = 0, j = 0; i < nr_2d_matches; ++i, j += 3)
    {
      xs.data()[j] = (double)correspondences[i].first.x;
      xs.data()[j + 1] = (double)correspondences[i].first.y;
      xps.data()[j] = (double)correspondences[i].second.x;
      xps.data()[j + 1] = (double)correspondences[i].second.y;
    }

    //relative pose estimation using the normalized eight point algorithm;
    if (!robustBinaryFitRansac<double, 3, 3, 9, 8, i_normalized_eight_point_ransac_hypogenfunc, i_relative_pose_costfunc, nullptr>(xs.data(), xps.data(), nr_2d_matches, f, nr_inlier_epi_geometry, inliers.data(), i_sqr(epi_dist_thre), false))
    {
      return 0; /*failed to estimate relative pose*/
    }

    i_indexed_shuffle3(xs.data(), xps.data(), inliers.data(), nr_2d_matches, nr_inlier_epi_geometry, false);
    i_shift_homogeneous3(xs.data(), nr_inlier_epi_geometry);
    i_shift_homogeneous3(xps.data(), nr_inlier_epi_geometry);

    /*optimize the fundamental matrix estimation*/
    i_force_fundamental(f);
    FundamentalMatrixOptimizer<double> f_polisher;
#ifdef _DEBUG
    f_polisher.setVerbose(true);
#endif
    f_polisher.optimize(f, xs.data(), xps.data(), nr_inlier_epi_geometry);
    f_polisher.getFundamental(f);

    /*re-compute epipolar geometry error*/
    epi_errors.resize(nr_inlier_epi_geometry, 0.0);
    for (i = 0; i < nr_inlier_epi_geometry; ++i)
    {
      j = i * 2;
      epi_errors.at(i) = i_symmetric_epiline_distance_fundamental(xs.data() + j, xps.data() + j, f);
    }

    /*adaptively compute the new error threshold - for further filtering out mis-matches*/
    error_mean = i_mean(epi_errors.data(), (int)epi_errors.size());
    error_sdv = i_sdv(epi_errors.data(), error_mean, (int)epi_errors.size());
    error_thresh = error_mean + 3.0 * error_sdv; /*fix to be mean error + 3 times standard deviation*/

    /*mark the final inliers as survivors*/
    for (i = 0; i < nr_inlier_epi_geometry; ++i)
    {
      if (epi_errors.at(i) <= error_thresh)
      {
        ++survivor_count;
        survivors.at(inliers[i]) = true;
      }
    }
    if (fundamental)
    {
      i_copy9(f, fundamental);
    }
    return survivor_count;
  }


  /*This routine verifies the epipolar geometry and filter out matching outliers*/
  template<typename InterestPointType, typename DescriptorType>
  inline int i_correspondences_epipolar_geometry_verification(const std::vector<InterestPointType> &ipts_1,
    const std::vector<InterestPointType> &ipts_2,
    const std::vector<Pair<int, int> > &correspondences,
    std::vector<bool>& survivors,
    double epi_dist_thre,
    double *fundamental = NULL)
  {
    if (correspondences.empty())
    {
      return 0;
    }
    int i, j, idx1, idx2, nr_inlier_epi_geometry = 0, survivor_count = 0;
    int nr_2d_matches = (int)correspondences.size();
    double f[9], error_mean, error_sdv, error_thresh;
    if (fundamental != NULL)
    {
      i_eye_3x3(fundamental);
    }
    survivors.resize(nr_2d_matches, false); /*initialization - no survivors*/
    std::vector<int> inliers(nr_2d_matches * 2);
    std::vector<double> xs(nr_2d_matches * 3, (double)1.0);
    std::vector<double> xps(nr_2d_matches * 3, (double)1.0);
    std::vector<double> epi_errors;

    /*start normalized eight points algorithm*/
    for (i = 0, j = 0; i < nr_2d_matches; ++i, j += 3)
    {
      idx1 = correspondences[i].first;
      idx2 = correspondences[i].second;

      xs.data()[j] = (double)ipts_1[idx1].x;
      xs.data()[j + 1] = (double)ipts_1[idx1].y;

      xps.data()[j] = (double)ipts_2[idx2].x;
      xps.data()[j + 1] = (double)ipts_2[idx2].y;
    }

    //relative pose estimation using the normalized eight point algorithm;
    if (!robustBinaryFitRansac<double, 3, 3, 9, 8, i_normalized_eight_point_ransac_hypogenfunc, i_relative_pose_costfunc, nullptr>(xs.data(), xps.data(), nr_2d_matches, f, nr_inlier_epi_geometry, inliers.data(), i_sqr(epi_dist_thre), false))
    {
      return 0; /*failed to estimate relative pose*/
    }

    i_indexed_shuffle3(xs.data(), xps.data(), inliers.data(), nr_2d_matches, nr_inlier_epi_geometry, false);
    i_shift_homogeneous3(xs.data(), nr_inlier_epi_geometry);
    i_shift_homogeneous3(xps.data(), nr_inlier_epi_geometry);

    /*optimize the fundamental matrix estimation*/
    i_force_fundamental(f);
    FundamentalMatrixOptimizer<double> f_polisher;
#ifdef _DEBUG
    f_polisher.setVerbose(true);
#endif
    f_polisher.optimize(f, xs.data(), xps.data(), nr_inlier_epi_geometry);
    f_polisher.getFundamental(f);

    /*re-compute epipolar geometry error*/
    epi_errors.resize(nr_inlier_epi_geometry, 0.0);
    for (i = 0; i < nr_inlier_epi_geometry; ++i)
    {
      j = i * 2;
      epi_errors.at(i) = i_symmetric_epiline_distance_fundamental(xs.data() + j, xps.data() + j, f);
    }

    /*adaptively compute the new error threshold - for further filtering out mis-matches*/
    error_mean = i_mean(epi_errors.data(), (int)epi_errors.size());
    error_sdv = i_sdv(epi_errors.data(), error_mean, (int)epi_errors.size());
    error_thresh = error_mean + 3.0 * error_sdv; /*fix to be mean error + 3 times standard deviation*/

    /*mark the final inliers as survivors*/
    for (i = 0; i < nr_inlier_epi_geometry; ++i)
    {
      if (epi_errors.at(i) <= error_thresh)
      {
        ++survivor_count;
        survivors.at(inliers[i]) = true;
      }
    }
    if (fundamental)
    {
      i_copy9(f, fundamental);
    }
    return survivor_count;
  }
}//namespace idl